#ifndef __MC_POINT_3_H__
#define __MC_POINT_3_H__

#include <iostream>
#include <sstream>
#include <cmath>
#include <vector>

using namespace std;

namespace mc{

template <typename T>
class Point3_{
    public:
        Point3_(T x = 0, T y = 0, T theta = 0){
            this->x = x;
            this->y = y;
            this->theta = theta;
        };
        Point3_() = default;

        void setX(T x)
        {
            this->x = x;
        }

        void setY(T y)
        {
            this->y = y;
        }

        void setTheta(T theta)
        {
            this->theta = theta;
        }

        string toString() const{
            std::stringstream ss;
            ss << "(" << this->x << ", " << this->y << ", " << this->theta << ")";
            return ss.str();
        }

        T operator[](size_t i) const
        {
            return i == 0 ? this->x : (i == 1 ? this->y : this->theta);
        }

        bool operator==(const Point3_& other)
        {
            return (this->x == static_cast<T>(other.x())) &&
                (this->y == static_cast<T>(other.y()) && (this->theta == static_cast<T>(other.theta())));
        }

        bool operator!=(const Point3_& other)
        {
            return !operator==(other);
        }

        T x, y, theta;    
};

}; //namespace
 
#endif